#include "sensortrayoutputelevator.h"

SensorTrayOutputElevator::SensorTrayOutputElevator(SensorTrayElevatorConfig *steConfig, QObject *parent)
    : Worker(WorkerName::STOE, parent), steConfig(steConfig)
{
    connect(&DiStateMonitor::getIns(), &DiStateMonitor::diStateChanged, this,
            &SensorTrayOutputElevator::onDiStateChanged, Qt::DirectConnection);
    connect(steConfig, &SensorTrayElevatorConfig::stoeMagTrayCountChanged, this,
            &SensorTrayOutputElevator::onMagTrayCountChanged, Qt::DirectConnection);

    mag1TrayState = new MagTrayState(this);
    mag1TrayStateStoreFile = new ConfigFile("STOEMag1TrayState", mag1TrayState,
                                            QString("%1/stoeMag1TrayState.json").arg(getWorkerConfigDir()));
    mag1TrayStateStoreFile->populate();
    mag2TrayState = new MagTrayState(this);
    mag2TrayStateStoreFile = new ConfigFile("STOEMag2TrayState", mag2TrayState,
                                            QString("%1/stoeMag2TrayState.json").arg(getWorkerConfigDir()));
    mag2TrayStateStoreFile->populate();
}

void SensorTrayOutputElevator::initEventHandle()
{
    stoeMagSNR1 = MotionElementContainer::getIns()->getItem<SCDI>(DiName::STOEMagSNR1);
    stoeMagSNR2 = MotionElementContainer::getIns()->getItem<SCDI>(DiName::STOEMagSNR2);
    stoeCylinder = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::STOECylinder);
    stoe = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::STOE);
}

void SensorTrayOutputElevator::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event)
    pushEvent(WorkerEvent("moveToNextInitTrayPos"));
}

void SensorTrayOutputElevator::stopEventHandle()
{
    stopEventHandled();
}

void SensorTrayOutputElevator::purgeEventHandle()
{
    purgeEventHandled();
}

void SensorTrayOutputElevator::homeWorker(bool moveToReadyPosAfterHome)
{
    Q_UNUSED(moveToReadyPosAfterHome)
    stoe->axis()->home();
}

bool SensorTrayOutputElevator::moveToNextInitTrayPos()
{
    checkIsInit();

    QMutexLocker tmpLocker(&locker);

    while (true)
    {
        if (hasAbortEvent())
        {
            return false;
        }
        if (steConfig->currentSTOEMag() == SensorTrayElevatorConfig::Unknown)
        {
            steConfig->setCurrentSTOEMag(SensorTrayElevatorConfig::Mag1);
        }
        if (steConfig->currentSTOEMag() == SensorTrayElevatorConfig::Mag1)
        {
            if (!stoeMagSNR1->get() || !hasInitTray(mag1TrayState))
            {
                steConfig->setCurrentSTOEMag(SensorTrayElevatorConfig::Mag2);
            }
        }
        if (steConfig->currentSTOEMag() == SensorTrayElevatorConfig::Mag2)
        {
            if (!stoeMagSNR2->get() || !hasInitTray(mag2TrayState))
            {
                steConfig->setCurrentSTOEMag(SensorTrayElevatorConfig::Unknown);
            }
        }
        if (steConfig->currentSTOEMag() == SensorTrayElevatorConfig::Unknown)
        {
            qint64 timeStampStart = QDateTime::currentMSecsSinceEpoch();
            DiStateMonitor::getIns().registerDi(DiName::STOEMagSNR1);
            DiStateMonitor::getIns().registerDi(DiName::STOEMagSNR2);
            UIOperation::getIns()->getUIResponse("Tip", tr("Please change sensor tray output elevator magazine."),
                                                 MsgBoxIcon::Information, YesBtn);
            qint64 timeStampEnd = QDateTime::currentMSecsSinceEpoch();
            DiStateMonitor::getIns().unRegisterDi(DiName::STOEMagSNR1);
            DiStateMonitor::getIns().unRegisterDi(DiName::STOEMagSNR2);

            if (magSNR1TurnToOnTimeStamp - magSNR1TurnToOffTimeStamp > 100 && magSNR1TurnToOnTimeStamp > timeStampStart
                && magSNR1TurnToOnTimeStamp < timeStampEnd)
            {
                mag1TrayState->resetMagTrayState(steConfig->stoeMagTrayCount(), MagTrayState::Init);
            }
            if (magSNR2TurnToOnTimeStamp - magSNR2TurnToOffTimeStamp > 100 && magSNR2TurnToOnTimeStamp > timeStampStart
                && magSNR2TurnToOnTimeStamp < timeStampEnd)
            {
                mag2TrayState->resetMagTrayState(steConfig->stoeMagTrayCount(), MagTrayState::Init);
            }
            continue;
        }
        break;
    }

    MagTrayState *currentMagTrayState
        = steConfig->currentSTOEMag() == SensorTrayElevatorConfig::Mag1 ? mag1TrayState : mag2TrayState;
    int initTrayIndex = -1;
    for (int i = 0; i < steConfig->stoeMagTrayCount(); i++)
    {
        if (currentMagTrayState->getMagTrayState(i) == MagTrayState::Init)
        {
            initTrayIndex = i;
            break;
        }
    }
    moveToMagTrayPosImpl(steConfig->currentSTOEMag(), initTrayIndex);
    return true;
}

void SensorTrayOutputElevator::setCurrentMagTrayState(MagTrayState::State state)
{
    MagTrayState *magTrayState
        = steConfig->currentSTOEMag() == SensorTrayElevatorConfig::Mag1 ? mag1TrayState : mag2TrayState;
    magTrayState->setMagTrayState(currentTrayIndex, state);
}

void SensorTrayOutputElevator::moveToMagTrayPos(int magIndex, int trayIndex)
{
    checkIsInit();

    validateIndex(trayIndex, steConfig->stoeMagTrayCount(), "Tray index out of range");
    moveToMagTrayPosImpl(magIndex, trayIndex);
}

void SensorTrayOutputElevator::resetMagTrayState(int magIndex)
{
    if (magIndex != 1 && magIndex != 2)
    {
        throw SilicolAbort(QString("Illegal magazine index: %1").arg(magIndex), EX_LOCATION);
    }
    MagTrayState *magTrayState = magIndex == 1 ? mag1TrayState : mag2TrayState;
    magTrayState->resetMagTrayState(steConfig->stoeMagTrayCount());
}

void SensorTrayOutputElevator::onDiStateChanged(QString diName, bool state)
{
    if (diName == DiName::STOEMagSNR1)
    {
        qint64 now = QDateTime::currentMSecsSinceEpoch();
        if (state)
        {
            magSNR1TurnToOnTimeStamp = now;
        }
        else
        {
            magSNR1TurnToOffTimeStamp = now;
        }
    }
    if (diName == DiName::STOEMagSNR2)
    {
        qint64 now = QDateTime::currentMSecsSinceEpoch();
        if (state)
        {
            magSNR2TurnToOnTimeStamp = now;
        }
        else
        {
            magSNR2TurnToOffTimeStamp = now;
        }
    }
}

void SensorTrayOutputElevator::onMagTrayCountChanged(int magTrayCount)
{
    mag1TrayState->resetMagTrayState(magTrayCount);
    mag2TrayState->resetMagTrayState(magTrayCount);
}

bool SensorTrayOutputElevator::hasInitTray(MagTrayState *magTrayState)
{
    for (int i = 0; i < steConfig->stoeMagTrayCount(); i++)
    {
        if (magTrayState->getMagTrayState(i) == MagTrayState::Init)
        {
            return true;
        }
    }
    return false;
}

void SensorTrayOutputElevator::moveToMagTrayPosImpl(int magIndex, int trayIndex)
{
    if (magIndex != 1 && magIndex != 2)
    {
        throw SilicolAbort(QString("Illegal magazine index: %1").arg(magIndex), EX_LOCATION);
    }
    if (trayIndex < 0 || trayIndex >= steConfig->stoeMagTrayCount())
    {
        throw SilicolAbort(QString("Illegal trayIndex: %1").arg(trayIndex), EX_LOCATION);
    }

    bool cylTargetState = (magIndex == 2);
    stoeCylinder->set(cylTargetState, false);

    SingleAxisPos *retrieveFirstTrayPos = stoe->getPos<SingleAxisPos>(SAxisPosName::RetrieveFirstTrayPos);
    SingleAxisPos *retrieveLastTrayPos = stoe->getPos<SingleAxisPos>(SAxisPosName::RetrieveLastTrayPos);
    double delta = (retrieveLastTrayPos->pos() - retrieveFirstTrayPos->pos()) / (steConfig->stoeMagTrayCount() - 1);
    double targetPos = retrieveFirstTrayPos->pos() + delta * trayIndex;
    stoe->axis()->absMove(targetPos, true, retrieveFirstTrayPos->precision());
    currentTrayIndex = trayIndex;
    stoeCylinder->wait(cylTargetState);
}
